#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <time.h>
#include <math.h>
#include <unistd.h>
#include <sys/time.h>

#include <ueye.h>

static double
currentsecond(void)
{
  struct timeval tv;
  gettimeofday(&tv, NULL);
  return (double) tv.tv_sec + 1e-6 * (double) tv.tv_usec;
}

static char *
timestamp(void)
{
  static char s[] = "YYYY-MM-DD HH:MM:SS";
  time_t t = time(NULL);
  struct tm *tm = localtime(&t);  
  strftime(s, sizeof(s), "%F %T", tm);
  return s;
}

static void
message(const char *message, ...)
{
  va_list ap;
  va_start(ap, message);
  fprintf(stderr, "%s: ", timestamp());
  vfprintf(stderr, message, ap);
  fprintf(stderr, "\n");
  va_end(ap);
}

static void
error(const char *message, ...)
{
  va_list ap;
  va_start(ap, message);
  fprintf(stderr, "%s: error: ", timestamp());
  vfprintf(stderr, message, ap);
  fprintf(stderr, "\n");
  va_end(ap);
  exit(1);
}

void
ueye_error(const char *when, HIDS cam)
{
  INT code;
  IS_CHAR *text;
  if (is_GetError(cam, &code, &text) != IS_SUCCESS)
    error("ueye error %s.", when);
  else
    error("ueye error %s: %s.", when, text);
}

static void
getpixelclockfrequency(HIDS c, double *frequency)
{
  message("getting pixel clock frequency.");
  UINT u; 
  if (is_PixelClock(c, IS_PIXELCLOCK_CMD_GET, &u, sizeof(u)) != IS_SUCCESS)
    ueye_error("while getting pixel clock frequency", c);
  message("pixel clock frequency is %u MHz.", u);
  if (frequency != NULL)
    *frequency = u * 1e6;
}

static void
setpixelclockfrequency(HIDS c, double frequency)
{
  message("setting pixel clock frequency.");
  UINT u = round(frequency / 1e6);
  if (is_PixelClock(c, IS_PIXELCLOCK_CMD_SET, &u, sizeof(u)) != IS_SUCCESS)
    ueye_error("while setting pixel clock frequency", c);
  getpixelclockfrequency(c, NULL);
}

static void
setframefrequency(HIDS c, double frequency)
{
  message("setting frame frequency to %.0f Hz.", frequency);
  double x;
  if (is_SetFrameRate(c, frequency, &x) != IS_SUCCESS)
    ueye_error("while setting frame frequency", c);
  message("frame frequency is %.0f Hz.", x);
  x = 0;
  if (is_Exposure(c, IS_EXPOSURE_CMD_SET_EXPOSURE, &x, sizeof(x)) != IS_SUCCESS)
    ueye_error("while setting exposure time", c);
  if (is_Exposure(c, IS_EXPOSURE_CMD_GET_EXPOSURE, &x, sizeof(x)) != IS_SUCCESS)
    ueye_error("while getting exposure time", c);
  message("exposure time is %.6f s.", x * 1e-3);
}

static void
getframefrequency(HIDS c, double *frequency)
{
  message("getting frame frequency.");
  double x;
  if (is_SetFrameRate(c, IS_GET_FRAMERATE, &x) != IS_SUCCESS)
    ueye_error("while getting frame frequency", c);
  message("frame frequency is %.0f Hz.", x);
  if (frequency != NULL)
    *frequency = x;
}

static void
getframefrequencyrange(HIDS c, double *minfrequency, double *maxfrequency)
{
  message("getting frame frequency range.");
  double x0, x1, x2;
  if (is_GetFrameTimeRange(c, &x0, &x1, &x2) != IS_SUCCESS)
    ueye_error("while getting frame time range", c);
  message("minimum frame time is %.6f s.", x0);
  message("maximum frame time is %.6f s.", x1);
  message("frame time increment is %.6f s.", x2);
  message("minimum frame frequency is %.0f Hz.", 1.0 / x1);
  message("maximum frame frequency is %.0f Hz.", 1.0 / x0);
  if (minfrequency != NULL)
    *minfrequency = 1.0 / x1;
  if (maxfrequency != NULL)
    *maxfrequency = 1.0 / x0;
}

static void
setaoi(HIDS c, int x, int y, int nx, int ny)
{
  message("setting AOI.");
  IS_RECT r = {
    .s32X      = x,
    .s32Y      = y,
    .s32Width  = nx,
    .s32Height = ny
  };
  if (is_AOI(c, IS_AOI_IMAGE_SET_AOI, &r, sizeof(r)) != IS_SUCCESS)
    ueye_error("while setting AOI", c);
  double maxfrequency;
  getframefrequencyrange(c, NULL, &maxfrequency);
  setframefrequency(c, maxfrequency);
  getframefrequency(c, NULL);
}

static void
getaoi(HIDS c, int *x, int *y, int *nx, int *ny)
{
  message("getting AOI.");
  IS_RECT r;
  if (is_AOI(c, IS_AOI_IMAGE_GET_AOI, &r, sizeof(r)) != IS_SUCCESS)
    ueye_error("while getting AOI", c);
  message("AOI origin is (%d,%d) and AOI size is (%d,%d).", 
      (int) r.s32X, (int) r.s32Y, 
      (int) r.s32Width, (int) r.s32Height);
  if (x != NULL)
    *x = r.s32X;
  if (y != NULL)
    *y = r.s32Y;
  if (nx != NULL)
    *nx = r.s32Width;
  if (ny != NULL)
    *ny = r.s32Height;
}

static void
acquire(HIDS c, int (*f)(unsigned long, int, int, const unsigned short **))
{  
  int nx, ny;
  getaoi(c, NULL, NULL, &nx, &ny);

  int nbuffers = 16;
  char *membuf[nbuffers];
  INT memid[nbuffers];

  message("allocating memory.");
  for (int i = 0; i < nbuffers; ++i) {
    if (is_AllocImageMem(c, nx, ny, 16, &membuf[i], &memid[i]) != IS_SUCCESS)
      ueye_error("while allocating image memory", c);
    if (is_AddToSequence(c, membuf[i], memid[i]) != IS_SUCCESS)
      ueye_error("while adding image memory to sequence", c);
  }
  
  const unsigned short *pixel[ny];
  INT lineincrement;
  if (is_GetImageMemPitch(c, &lineincrement) != IS_SUCCESS)
      ueye_error("while determining line increment", c); 

  message("acquiring.");
  if (is_CaptureVideo(c, IS_WAIT) != IS_SUCCESS)
    ueye_error("while acquiring", c);
  is_EnableEvent(c, IS_SET_EVENT_FRAME);
  unsigned long j = 0;
  for (;;) {
    if (is_WaitEvent(c, IS_SET_EVENT_FRAME, 1000) == IS_SUCCESS) {
      char *lastmembuf;
      if (is_GetActSeqBuf(c, NULL, NULL, &lastmembuf) != IS_SUCCESS)
         ueye_error("while getting image memory", c);
      int i;
      for (i = 0; i < nbuffers; ++i) {
        if (lastmembuf == membuf[i])
          break;
      }
      if (i == nbuffers)
        error("unexpected last memory buffer pointer.");
      UEYEIMAGEINFO info;
      if (is_GetImageInfo(c, memid[i], &info, sizeof (info)) == IS_SUCCESS) {
	//message("timestamp = %llu", info.u64TimestampDevice);
      }
      for (int iy = 0; iy < ny; ++iy)
        pixel[iy] = (unsigned short *) (lastmembuf + iy * lineincrement);
      if (f(j, nx, ny, pixel) == 0)
        break;
      ++j;
    }
  }
  message("finished acquiring.");
  is_DisableEvent(c, IS_SET_EVENT_FRAME);  
  if (is_StopLiveVideo(c, IS_WAIT) != IS_SUCCESS)
    ueye_error("while stopping acquiring", c);
  
  // Pass timestamps.
  // Need to deal with line increments.
  
  message("freeing memory.");
  for (int i = 0; i < nbuffers; ++i)
    if (is_FreeImageMem(c, membuf[i], memid[i]) != IS_SUCCESS)
      ueye_error("while freeing image memory", c);
  
  message("finished acquiring.");
}

HIDS
opendetector(void)
{
  double start = currentsecond();
  message("opening detector.");
  HIDS c = 0;
  int i = is_InitCamera(&c, NULL);
  if (i == IS_STARTER_FW_UPLOAD_NEEDED) {
    c |= IS_ALLOW_STARTER_FW_UPLOAD;
    i = is_InitCamera(&c, NULL);;
  }
  if (i != IS_SUCCESS)
    ueye_error("while opening detector", c);
  message("camera is %d.", (int) c);
  message("checking display mode.");
  int dm = is_SetDisplayMode(c, IS_GET_DISPLAY_MODE);
  if (dm != IS_SET_DM_DIB)
    error("display mode is not IS_SET_DM_DIB.");
  message("setting color mode to 16-bit raw.");
  if (is_SetColorMode(c, IS_CM_SENSOR_RAW16) != IS_SUCCESS)
    ueye_error("while setting color mode", c);  
  message("setting gain boost to off.");
  if (is_SetGainBoost(c, IS_SET_GAINBOOST_OFF) != IS_SUCCESS)
    ueye_error("while setting gain boost", c);  
  message("setting hardware gain.");
  if (is_SetHardwareGain(c, 0, 0, 0, 0) != IS_SUCCESS)
    ueye_error("while setting hardware gain", c);  
  getpixelclockfrequency(c, NULL);
  int x, y, nx, ny;
  getaoi(c, &x, &y, &nx, &ny);
  setaoi(c, x, y, nx, ny);
  double end = currentsecond();
  message("finished opening.");
  message("opening took %.1f s.", end - start);
  return c;
}

void
closedetector(HIDS c)
{
  message("closing.");
  if (is_ExitCamera(c) != IS_SUCCESS)
    ueye_error("while closing", c);
}

#if 0
#define nframes 2400
#define SX 0
#define SY 0
#define NX 1280
#define NY 1024
#else
#define nframes 24000
#define SX 692
#define SY 500
#define NX 192
#define NY 96
#endif
static double z[NY][NX];

void
writefits(const char *filename)
{
  FILE *fp = fopen(filename, "wb");
  if (fp == 0)
    error("unable to open FITS file \"%s\".", filename);
  fprintf(fp, "%-8.8s= %20.20s%50.50s", "SIMPLE", "T", "");
  fprintf(fp, "%-8.8s= %20d%50.50s", "BITPIX", -64, "");
  fprintf(fp, "%-8.8s= %20d%50.50s", "NAXIS", 2, "");
  fprintf(fp, "%-8.8s= %20d%50.50s", "NAXIS1", NX, "");
  fprintf(fp, "%-8.8s= %20d%50.50s", "NAXIS2", NY, "");
  fprintf(fp, "%-80.80s", "END");
  for (int i = 0; i < 30; ++i)
    fprintf(fp, "%-80.80s", "");
  for (int iy = 0; iy < NY; ++iy)
    for (int ix = 0; ix < NX; ++ix) {
      unsigned char *u = (void *) &z[iy][ix];
      for (int i = 0; i < sizeof(double); ++i)
        putc(u[sizeof(double) - 1 - i], fp);
    }
  size_t n = sizeof(double) * (size_t) NX * (size_t) NY;
  for (size_t i = n; i % 2880 != 0; ++i)
    putc(0, fp);
  fclose(fp);
}

int
processframe(unsigned long i, int nx, int ny, const unsigned short **mem)
{
  for (int iy = 0; iy < ny; ++iy)
    for (int ix = 0; ix < nx; ++ix)
      z[iy][ix] += mem[iy][ix];
  return i < nframes;
}

int
main()
{
  message("starting.");
  HIDS c = opendetector();
  setpixelclockfrequency(c, 48e6);
  setaoi(c, SX, SY, NX, NY);

  for (int iy = 0; iy < NY; ++iy)
    for (int ix = 0; ix < NX; ++ix)
      z[iy][ix] = 0.0;

  double start = currentsecond();
  acquire(c, processframe);
  double end = currentsecond();
  message("achieved frame frequency of %.0f Hz.", 
    (double) nframes / (end - start));
  for (int iy = 0; iy < NY; ++iy)
    for (int ix = 0; ix < NX; ++ix)
      z[iy][ix] /= (double) nframes;
  
  writefits("wfs.fits");  
  closedetector(c);
  message("exiting.");
  return 0;
}
